/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2024-05-12     liuchao       the first version
 */
#define LOG_TAG "rotate"
#include <ulog.h>
#include "rotate.h"

#include <rtthread.h>
#include <stdlib.h>
#include "utils.h"

#include "drv_relay.h"
#include "drv_btn.h"
#include "drv_lock_switch.h"
#include "drv_sensor.h"
#include "led_beep.h"
#include "sleep.h"
#include "aliyun.h"

static int s_rotating = 0;

// static rt_mailbox_t s_mb = NULL;

typedef enum
{
    ROTATE_STATE_PUTTER_BACK = 1,
    ROTATE_STATE_PUTTER_PUSH = 1 << 1,
    ROTATE_STATE_GEAR_LOCK = 1 << 2,
    ROTATE_STATE_GEAR_UNLOCK = 1 << 3,
    ROTATE_STATE_PUTTER_STOP = 1 << 4,
    ROTATE_STATE_GEAR_STOP = 1 << 5,
    ROTATE_STATE_PUTTER_MIN_REACH = 1 << 6,
    ROTATE_STATE_PUTTER_MAX_REACH = 1 << 7,
    ROTATE_STATE_HY_STOP = 1 << 8,
    ROTATE_STATE_ALL_STOP = 1 << 31,
} rotate_state_e;

static int s_log_count = 0;
static float s_prev_axial = 0.0;
static rt_uint32_t s_prev_state = 0;

static void rotate_entry(void *arg)
{
    rt_thread_mdelay(10000);
    rt_tick_t now_ms = rt_tick_get_millisecond();
    rt_tick_t prev_ms = now_ms;
    int delta_ms = 0;
    int auto_mode_go_back_count_down = 0;
    int auto_mode_release_pressure_count_down = 0;
    const int loop_delay = 100;
    while (1)
    {
        s_log_count++;
        prev_ms = now_ms;
        now_ms = rt_tick_get_millisecond();
        delta_ms = now_ms - prev_ms;
        if (delta_ms < 0)
        {
            LOG_W("delta ms less than 0, %d", delta_ms);
            delta_ms = loop_delay;
        }

        if (s_log_count % 5 == 0)
        {
            LOG_D("delta ms, %d", delta_ms);
        }

        rt_uint32_t next_state = 0;
        rt_int8_t skip_fsm = 0;
        int auto_mode = btn_state(BTN_AUTO);
        int manual_mode = btn_state(BTN_MANUAL);
        int push_btn = btn_state(BTN_PUSH);
        int back_btn = btn_state(BTN_BACK);
        int lock_switch_2mm_state = lock_switch_state(LOCK_SWITCH_2MM);
        int lock_switch_4mm_state = lock_switch_state(LOCK_SWITCH_4MM);
        float auto_mode_baseline_v = auto_mode_baseline();
        float auto_mode_percentage_v = auto_mode_percentage();
        float putter_min = configuration_putter_distance_min();
        float putter_max = configuration_putter_distance_max();
        float bat_low_thd = battery_low_threshold_get();
        int auto_mode_pause_switch = pause_switch_get();
        int auto_mode_go_back_seconds = go_back_seconds_get();
        int auto_mode_release_pressure_seconds = release_pressure_get();
        int update_inverval = auto_mode_update_inverval();

        float axial = 0;
        float putter_distance = 0;
        float bat_voltage = 0;

        if (manual_mode || (auto_mode && auto_mode_pause_switch == 0))
        {
            /*
             * for sensor read
             */
            relay_on(RELAY_PERIPH_PWR);
        }

        axial = sensor_axial(SENSOR_OIL_PRESSURE);
        putter_distance = sensor_distance(SENSOR_PUTTER_DISTANCE);
        bat_voltage = sensor_voltage(SENSOR_BATTERY);

        if (bat_voltage < bat_low_thd)
        {
            auto_mode_pause_switch = 1;
            if (pause_switch_get() == 0)
            {
                pause_switch_set(1);
                aliyun_pub_property_int32("PauseSwitch", 1);
            }
        }

        if (manual_mode)
        {
            if (push_btn || back_btn)
            {
                s_rotating = 1;
                relay_on(RELAY_PERIPH_PWR);
            }
            /*
             * auto mode release pressure and go back finish now
             */
            auto_mode_release_pressure_count_down = 0;
            auto_mode_go_back_count_down = 0;
        }
        else if (auto_mode)
        {
            if (s_log_count % 5 == 0)
            {
                LOG_D("auto mode pause switch: %d, baseline: %f, percentage: %f, %f\n", auto_mode_pause_switch,
                        auto_mode_baseline_v, auto_mode_percentage_v, axial);
                LOG_D("auto mode release pressure: %d, %d, go back: %d, %d\n", auto_mode_release_pressure_seconds,
                        auto_mode_release_pressure_count_down, auto_mode_go_back_seconds, auto_mode_go_back_count_down);
                LOG_D("auto mode battery voltage: %f, thd: %f\n", bat_voltage, bat_low_thd);
            }
            if (auto_mode_pause_switch)
            {
                if (update_inverval > AUTO_MODE_UPDATE_INTERVAL_THD)
                {
                    /*
                     * nothing to do
                     */
                }
                else if (auto_mode_release_pressure_count_down <= 0 && auto_mode_release_pressure_seconds > 0)
                {
                    if (auto_mode_baseline_v > 20 && axial > (auto_mode_baseline_v * (auto_mode_percentage_v + 1)))
                    {
                        auto_mode_release_pressure_count_down = auto_mode_release_pressure_seconds * 1000;
                        LOG_I("auto mode release pressure: %d", auto_mode_release_pressure_count_down);
                    }
                }
                else if (auto_mode_go_back_count_down <= 0 && auto_mode_go_back_seconds > 0)
                {
                    if (auto_mode_baseline_v >= 1.234 && auto_mode_baseline_v < 1.235 && axial <= 1000.0)
                    {
                        auto_mode_go_back_count_down = auto_mode_go_back_seconds * 1000;
                        LOG_I("auto mode go back: %d", auto_mode_go_back_count_down);
                    }
                }
            }
        }

        if (auto_mode_release_pressure_count_down >= 0)
        {
            auto_mode_release_pressure_count_down -= delta_ms;
            if (auto_mode_release_pressure_count_down <= 0 && auto_mode_release_pressure_seconds > 0)
            {
                LOG_I("auto mode release pressure done");
                release_pressure_set(0);
            }
        }

        if (auto_mode_go_back_count_down >= 0)
        {
            auto_mode_go_back_count_down -= delta_ms;
            if (auto_mode_go_back_count_down <= 0 && auto_mode_go_back_seconds > 0)
            {
                LOG_I("auto mode go back done");
                go_back_seconds_set(0);
            }
        }

        rt_thread_mdelay(loop_delay);

        if (s_log_count == 1)
        {
            LOG_D("auto %d manual %d push %d back %d 2mm %d 4mm %d", auto_mode, manual_mode, push_btn, back_btn,
                    lock_switch_2mm_state, lock_switch_4mm_state);
        }

        /*
         * compute next state
         */
        if (manual_mode)
        {
            if (back_btn)
            {
                LWM_BITMASK_SET(next_state, ROTATE_STATE_PUTTER_BACK);
            }
            else if (push_btn)
            {
                LWM_BITMASK_SET(next_state, ROTATE_STATE_PUTTER_PUSH);
            }
            else
            {
                next_state = ROTATE_STATE_ALL_STOP;
            }
        }
        else if (auto_mode)
        {
            if (s_log_count == 0)
            {
                LOG_D("axial: %f", axial);
            }
            if (s_prev_axial == 0)
            {
                s_prev_axial = axial;
            }

#if 0
            if (abs(axial - s_prev_axial) > 100)
            {
                skip_fsm = 1;
                LOG_D("axial skip, cur: %f, prev: %f", axial, s_prev_axial);
            }
            else
#endif
            {
                if (auto_mode_pause_switch && update_inverval > AUTO_MODE_UPDATE_INTERVAL_THD)
                {
                    next_state = ROTATE_STATE_ALL_STOP;
                }
                else if (auto_mode_release_pressure_count_down > 0)
                {
                    if (axial <= (auto_mode_baseline_v * (1 + auto_mode_percentage_v)))
                    {
                    }
                    else
                    {
                        LWM_BITMASK_SET(next_state, ROTATE_STATE_PUTTER_BACK);
                    }
                }
                else if (auto_mode_go_back_count_down > 0)
                {
                    LWM_BITMASK_SET(next_state, ROTATE_STATE_PUTTER_BACK);
                }
                else if (auto_mode_baseline_v == 0 || auto_mode_percentage_v == 0 || auto_mode_pause_switch == 1)
                {
                    next_state = ROTATE_STATE_ALL_STOP;
                }
                else
                {
                    float upper = auto_mode_baseline_v * (float) (1 + auto_mode_percentage_v);
                    float lower = auto_mode_baseline_v * (float) (1 - auto_mode_percentage_v);

                    if (axial > upper)
                    {
                        rt_thread_mdelay(200);
                        axial = sensor_axial(SENSOR_OIL_PRESSURE);
                        if (axial > upper)
                        {
                            next_state = ROTATE_STATE_ALL_STOP;
                            axial_set_is_met(1);
                            LOG_D("axial %f > %f", axial, upper);
                        }
                        else
                        {
                            LOG_D("skip axial %f > %f", axial, upper);
                            skip_fsm = 1;
                        }
                    }
                    else if (axial < lower)
                    {
                        if (s_log_count % 5 == 0)
                        {
                            LOG_D("axial %f < %f", axial, lower);
                        }
                        if (axial_is_met())
                        {
                            time_t met_ts = axial_met_ts();
                            time_t now = time(NULL);
                            if (now - met_ts < 3)
                            {
                                next_state = ROTATE_STATE_ALL_STOP;
                            }
                        }
                        LWM_BITMASK_SET(next_state, ROTATE_STATE_PUTTER_PUSH);
                    }
                    else
                    {
                        if (LWM_BITMASK_CHECK_ANY(s_prev_state, ROTATE_STATE_PUTTER_PUSH))
                        {
                            if (s_log_count % 5 == 0)
                            {
                                LOG_D("axial maintain %f < %f < %f", lower, axial, upper);
                            }
                            LWM_BITMASK_SET(next_state, ROTATE_STATE_PUTTER_PUSH);
                        }
                        else
                        {
                            if (s_log_count % 5 == 0)
                            {
                                LOG_D("axial maintain stop %f < %f < %f", lower, axial, upper);
                            }
                            axial_set_is_met(1);
                            next_state = ROTATE_STATE_ALL_STOP;
                        }
                    }
                }
            }

            s_prev_axial = axial;
        }

        if (lock_switch_2mm_state && lock_switch_4mm_state)
        {
            if (LWM_BITMASK_CHECK_ANY(next_state, ROTATE_STATE_PUTTER_BACK))
            {
                LWM_BITMASK_SET(next_state, ROTATE_STATE_PUTTER_STOP);
                LWM_BITMASK_SET(next_state, ROTATE_STATE_GEAR_LOCK);
            }
            else if (LWM_BITMASK_CHECK_ANY(next_state, ROTATE_STATE_PUTTER_PUSH))
            {
                LWM_BITMASK_SET(next_state, ROTATE_STATE_GEAR_STOP);
            }
            if (s_log_count % 5 == 0)
            {
                LOG_D("attach, 0x%x", next_state);
            }
        }
        else if (!lock_switch_2mm_state && !lock_switch_4mm_state)
        {
            if (LWM_BITMASK_CHECK_ANY(next_state, ROTATE_STATE_PUTTER_BACK))
            {
                LWM_BITMASK_SET(next_state, ROTATE_STATE_GEAR_STOP);
            }
            else if (LWM_BITMASK_CHECK_ANY(next_state, ROTATE_STATE_PUTTER_PUSH))
            {
                LWM_BITMASK_SET(next_state, ROTATE_STATE_PUTTER_STOP);
                LWM_BITMASK_SET(next_state, ROTATE_STATE_GEAR_UNLOCK);
            }
            if (s_log_count % 5 == 0)
            {
                LOG_D("detach, 0x%x", next_state);
            }
        }
        else
        {
            if (LWM_BITMASK_CHECK_ANY(next_state, ROTATE_STATE_PUTTER_BACK))
            {
                LWM_BITMASK_SET(next_state, ROTATE_STATE_GEAR_LOCK);
            }
            else if (LWM_BITMASK_CHECK_ANY(next_state, ROTATE_STATE_PUTTER_PUSH))
            {
                LWM_BITMASK_SET(next_state, ROTATE_STATE_GEAR_UNLOCK);
            }
        }

#if 0
        if (lock_switch_4mm_state)
        {
            if (LWM_BITMASK_CHECK_ANY(next_state, ROTATE_STATE_PUTTER_BACK))
            {
                LWM_BITMASK_SET(next_state, ROTATE_STATE_GEAR_LOCK);
            }
            else if (LWM_BITMASK_CHECK_ANY(next_state, ROTATE_STATE_PUTTER_PUSH))
            {
            }
        }
        else
        {
            if (LWM_BITMASK_CHECK_ANY(next_state, ROTATE_STATE_PUTTER_BACK))
            {
                LWM_BITMASK_SET(next_state, ROTATE_STATE_GEAR_STOP);
            }
            else if (LWM_BITMASK_CHECK_ANY(next_state, ROTATE_STATE_PUTTER_PUSH))
            {
                LWM_BITMASK_SET(next_state, ROTATE_STATE_PUTTER_STOP);
            }
            if (s_log_count % 5 == 0)
            {
                LOG_D("4mm detach, 0x%x", next_state);
            }
        }
#endif

#if 1
        /*
         * putter safe guard
         */
        if (LWM_BITMASK_CHECK_ANY(s_prev_state, ROTATE_STATE_PUTTER_MIN_REACH))
        {
            if (putter_distance < (putter_min + 5))
            {
                if (s_log_count % 5 == 0)
                {
                    LOG_D("putter distance hysteresis: %f < %f", putter_distance, putter_min + 5);
                }
                LWM_BITMASK_SET(next_state, ROTATE_STATE_PUTTER_MIN_REACH | ROTATE_STATE_ALL_STOP);
            }
        }
        if (LWM_BITMASK_CHECK_ANY(s_prev_state, ROTATE_STATE_PUTTER_MAX_REACH))
        {
            if (putter_distance > (putter_max - 5))
            {
                if (s_log_count % 5 == 0)
                {
                    LOG_D("putter distance hysteresis: %f > %f", putter_distance, putter_max - 5);
                }
                LWM_BITMASK_SET(next_state, ROTATE_STATE_PUTTER_MAX_REACH | ROTATE_STATE_ALL_STOP);
            }
        }

        if (LWM_BITMASK_CHECK_ANY(next_state, ROTATE_STATE_PUTTER_BACK) && (putter_distance < putter_min))
        {
            if (s_log_count % 5 == 0 || !LWM_BITMASK_CHECK_ANY(s_prev_state, ROTATE_STATE_PUTTER_MIN_REACH))
            {
                LOG_D("putter distance: %f < %f", putter_distance, putter_min);
            }
            LWM_BITMASK_SET(next_state, ROTATE_STATE_PUTTER_MIN_REACH | ROTATE_STATE_ALL_STOP);
        }
        if (LWM_BITMASK_CHECK_ANY(next_state, ROTATE_STATE_PUTTER_PUSH) && (putter_distance > putter_max))
        {
            if (s_log_count % 5 == 0 || !LWM_BITMASK_CHECK_ANY(s_prev_state, ROTATE_STATE_PUTTER_MAX_REACH))
            {
                LOG_D("putter distance: %f > %f", putter_distance, putter_max);
            }
            LWM_BITMASK_SET(next_state, ROTATE_STATE_PUTTER_MAX_REACH | ROTATE_STATE_ALL_STOP);
        }

        if (LWM_BITMASK_CHECK_ANY(next_state, ROTATE_STATE_PUTTER_MAX_REACH))
        {
            if (LWM_BITMASK_CHECK_ANY(next_state, ROTATE_STATE_PUTTER_BACK))
            {
                LWM_BITMASK_CLEAR(next_state, ROTATE_STATE_PUTTER_MIN_REACH | ROTATE_STATE_ALL_STOP);
            }
        }
        if (LWM_BITMASK_CHECK_ANY(next_state, ROTATE_STATE_PUTTER_MIN_REACH))
        {
            if (LWM_BITMASK_CHECK_ANY(next_state, ROTATE_STATE_PUTTER_PUSH))
            {
                LWM_BITMASK_CLEAR(next_state, ROTATE_STATE_PUTTER_MAX_REACH | ROTATE_STATE_ALL_STOP);
            }
        }

#endif
        if (s_log_count % 5 == 0)
        {
            LOG_D("next state, 0x%x", next_state);
        }

        if (s_log_count == 20)
        {
            s_log_count = 0;
        }

        rotate_set_handled(1);
        if (skip_fsm)
        {
            continue;
        }
        s_prev_state = next_state;
        // rt_mb_send(s_mb, next_state);

        /*
         * Rotate FSM
         */
        if (LWM_BITMASK_CHECK_ANY(next_state, ROTATE_STATE_ALL_STOP))
        {
            int delay = 0;
            if (relay_state(RELAY_HY_START) == RELAY_STATE_ON)
            {
                //delay = 3000;
            }
            relay_off(RELAY_PUSH);
            relay_off(RELAY_BACK);
            relay_off(RELAY_LOCK);
            relay_off(RELAY_UNLOCK);
            if (delay)
            {
                rt_thread_mdelay(delay);
            }
            relay_off(RELAY_HYDRAULIC);
            relay_off(RELAY_HY_START);
            relay_off(RELAY_SAFE);
#if 0
            if (!publishing())
            relay_off(RELAY_PERIPH_PWR);
#endif
            led_beep(PANEL_LED_YELLOW, LED_NOTIFY_MODE_OFF);
            led_beep(PANEL_BEE, LED_NOTIFY_MODE_OFF);
            s_rotating = 0;
            continue;
        }
        s_rotating = 1;

        if (LWM_BITMASK_CHECK_ALL(next_state, ROTATE_STATE_HY_STOP))
        {
#if 0
            led_beep(PANEL_LED_YELLOW, LED_NOTIFY_MODE_ALARM_HY_STOP);
            led_beep(PANEL_BEE, LED_NOTIFY_MODE_ALARM_HY_STOP);
#endif
        }
        else
        {
            led_beep(PANEL_LED_YELLOW, LED_NOTIFY_MODE_ALARM_2);
            led_beep(PANEL_BEE, LED_NOTIFY_MODE_ALARM_2);
        }

        if (LWM_BITMASK_CHECK_ANY(next_state,
                ROTATE_STATE_GEAR_LOCK | ROTATE_STATE_GEAR_UNLOCK | ROTATE_STATE_PUTTER_BACK
                        | ROTATE_STATE_PUTTER_PUSH))
        {
            int delay = 0;
            if (relay_state(RELAY_HY_START) == RELAY_STATE_OFF)
            {
                // delay = 3000;
            }
            if (LWM_BITMASK_CHECK_ALL(next_state, ROTATE_STATE_HY_STOP))
            {
                relay_off(RELAY_HYDRAULIC);
                relay_off(RELAY_HY_START);
                relay_off(RELAY_SAFE);
            }
            else
            {
                relay_on(RELAY_HYDRAULIC);
                relay_on(RELAY_HY_START);
                relay_on(RELAY_SAFE);
            }
            if (delay)
            {
                rt_thread_mdelay(delay);
            }
        }
        else
        {
            relay_off(RELAY_HYDRAULIC);
            relay_off(RELAY_HY_START);
            relay_off(RELAY_SAFE);
        }

        if (LWM_BITMASK_CHECK_ANY(next_state, ROTATE_STATE_PUTTER_STOP))
        {
            relay_off(RELAY_BACK);
            relay_off(RELAY_PUSH);
        }
        else
        {
            if (LWM_BITMASK_CHECK_ANY(next_state, ROTATE_STATE_PUTTER_BACK))
            {
                relay_on(RELAY_BACK);
            }
            else if (LWM_BITMASK_CHECK_ANY(next_state, ROTATE_STATE_PUTTER_PUSH))
            {
                relay_on(RELAY_PUSH);
            }
        }

        if (LWM_BITMASK_CHECK_ANY(next_state, ROTATE_STATE_GEAR_STOP))
        {
            relay_off(RELAY_LOCK);
            relay_off(RELAY_UNLOCK);
        }
        else
        {
            if (LWM_BITMASK_CHECK_ANY(next_state, ROTATE_STATE_GEAR_LOCK))
            {
                if (s_log_count % 5 == 0)
                {
                    LOG_D("gear lock");
                }
                relay_on(RELAY_LOCK);
            }
            else if (LWM_BITMASK_CHECK_ANY(next_state, ROTATE_STATE_GEAR_UNLOCK))
            {
                if (s_log_count % 5 == 0)
                {
                    LOG_D("gear unlock");
                }
                relay_on(RELAY_UNLOCK);
            }
        }
    }
}

int rotating()
{
    return s_rotating;
}

static rt_tick_t s_hy_ms = 0;
static rt_mutex_t s_hy_ms_mutex;
static rt_tick_t s_run_seconds = 0;
static rt_mutex_t s_run_seconds_mutex;
static void pm_entry(void *arg)
{
    int prev_state = RELAY_STATE_END;
    s_hy_ms = configuration_get_uint32(CONFIGURATION_HY_RUN_MS);
    rt_uint32_t count = 0;
    while (1)
    {
        rt_thread_mdelay(100);

#if 0
        if (count % 10 == 0)
        {
            rt_mutex_take(s_run_seconds_mutex, RT_WAITING_FOREVER);
            s_run_seconds += 1;
            rt_mutex_release(s_run_seconds_mutex);
            if (count % 1000 == 0)
            configuration_set_uint32(CONFIGURATION_RUN_SECONDS, run_seconds());
        }
#endif

        int cur_state = relay_state(RELAY_HY_START);
        if (cur_state == RELAY_STATE_ON)
        {
            rt_mutex_take(s_hy_ms_mutex, RT_WAITING_FOREVER);
            s_hy_ms += 100;
            rt_mutex_release(s_hy_ms_mutex);
        }
        if (prev_state == RELAY_STATE_END)
        {
            prev_state = cur_state;
        }
        else
        {
            if (cur_state == RELAY_STATE_OFF && prev_state == RELAY_STATE_ON)
            {
                configuration_set_uint32(CONFIGURATION_HY_RUN_MS, hy_ms());
            }
            prev_state = cur_state;
        }

        count++;
    }
}

rt_tick_t hy_ms()
{
    rt_tick_t ret = 0;
    rt_mutex_take(s_hy_ms_mutex, RT_WAITING_FOREVER);
    ret = s_hy_ms;
    rt_mutex_release(s_hy_ms_mutex);
    return ret;
}

rt_uint32_t run_seconds()
{
    rt_tick_t ret = 0;
    rt_mutex_take(s_run_seconds_mutex, RT_WAITING_FOREVER);
    ret = s_run_seconds;
    rt_mutex_release(s_run_seconds_mutex);
    return ret;
}

static int rotate_init()
{
    ulog_tag_lvl_filter_set(LOG_TAG, LOG_LVL_INFO);
#if 0
    s_mb = rt_mb_create("rot_mb", 64, RT_IPC_FLAG_FIFO);
    rt_thread_t rotate_th = rt_thread_create("rot_cntl", rotate_entry, NULL, 512, 3, 1000);
    rt_thread_startup(rotate_th);
#endif

#if 0
    rt_timer_t rotate_timer = rt_timer_create("rot_hdl", rotate_handler, NULL, rt_tick_from_millisecond(100),
            RT_TIMER_FLAG_PERIODIC | RT_TIMER_FLAG_SOFT_TIMER);
    rt_timer_start(rotate_timer);
#endif

    s_hy_ms_mutex = rt_mutex_create("hy_ms", RT_IPC_FLAG_FIFO);
    s_run_seconds_mutex = rt_mutex_create("run_secs", RT_IPC_FLAG_FIFO);

    rt_thread_t t = rt_thread_create("rotate", rotate_entry, NULL, 2048, 10, 1000);
    rt_thread_startup(t);

    t = rt_thread_create("timing", pm_entry, NULL, 2048, 30, 1000);
    rt_thread_startup(t);
    return RT_EOK;
}
INIT_APP_EXPORT(rotate_init);
